#include <Arduino.h>
#include <Wire.h>

#define DEBUG_MODE 1
#define PCA9685_ADDR1 0x40
#define PCA9685_ADDR2 0x41
#define SERVO_MIN_PULSE 500
#define SERVO_MAX_PULSE 2500

// 32通道角度配置（初始值）
uint8_t channelAngles[32] = {
  /* PCA9685 #1 (0x40) */
  0,   15, 30, 45, 60, 75, 90, 105,   // CH0-7
  120, 135, 150, 165, 180, 10, 25, 40, // CH8-15
  
  /* PCA9685 #2 (0x41) */
  5,   35, 65, 95, 125, 155, 20, 50,  // CH16-23
  80,  110, 140, 170, 55, 85, 115, 145 // CH24-31
};

// 函数声明
void setupPCA9685(uint8_t addr);
void setPWMFreq(uint8_t addr, float freq);
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle);
void updateAllChannels();
void printChannelGrid();
void validateData(uint8_t addr, uint8_t ch, uint8_t &angle, uint16_t &pulse);

void setup() {
  #if DEBUG_MODE
  Serial.begin(115200);
  while(!Serial); // 等待串口连接
  Serial.println("\n32 Channel Servo Controller v2.0");
  #endif

  Wire.begin(21, 22);
  Wire.setClock(100000);
  
  setupPCA9685(PCA9685_ADDR1);
  setupPCA9685(PCA9685_ADDR2);
  
  #if DEBUG_MODE
  Serial.println("System Ready");
  Serial.println("====== Initial Setup =======");
  printChannelGrid();
  #endif
}

void loop() {
  static uint32_t lastUpdate = 0;
  
  if(millis() - lastUpdate >= 1000) {
    lastUpdate = millis();
    
    // 角度更新（每个通道+5°）
    for(int i=0; i<32; i++) {
      channelAngles[i] = (channelAngles[i] + 5) % 181;
    }
    
    updateAllChannels();
    
    #if DEBUG_MODE
    Serial.printf("\n[%lu ms] Update Cycle\n", millis());
    printChannelGrid();
    #endif
  }
}

// 数据校验函数
void validateData(uint8_t addr, uint8_t ch, uint8_t &angle, uint16_t &pulse) {
  pulse = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  
  if(angle > 180 || pulse < SERVO_MIN_PULSE || pulse > SERVO_MAX_PULSE) {
    #if DEBUG_MODE
    Serial.printf("[WARN] 0x%02X CH%d Invalid: %d° %dμs -> ", addr, ch, angle, pulse);
    #endif
    
    angle = constrain(angle, 0, 180);
    pulse = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
    
    #if DEBUG_MODE
    Serial.printf("Corrected to %d° %dμs\n", angle, pulse);
    #endif
  }
}

// 更新所有通道
void updateAllChannels() {
  for(uint8_t ch=0; ch<16; ch++) {
    uint16_t pulse1 = 0, pulse2 = 0;
    validateData(PCA9685_ADDR1, ch, channelAngles[ch], pulse1);
    validateData(PCA9685_ADDR2, ch, channelAngles[ch+16], pulse2);
    
    setServoAngle(PCA9685_ADDR1, ch, channelAngles[ch]);
    setServoAngle(PCA9685_ADDR2, ch, channelAngles[ch+16]);
  }
}

// 表格化打印
void printChannelGrid() {
  // 表头
  Serial.println("+--------+--------+--------+--------+");
  Serial.println("| Device | Channel| Angle | Pulse |");
  Serial.println("+--------+--------+--------+--------+");

  // 打印PCA9685 #1
  for(int i=0; i<16; i++) {
    uint16_t pulse = map(channelAngles[i], 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
    Serial.printf("| 0x%02X  | CH%-2d  | %-3d   | %-4d |\n", 
                PCA9685_ADDR1, i, channelAngles[i], pulse);
    if((i+1)%4 == 0) Serial.println("+--------+--------+--------+--------+");
  }

  // 分隔线
  Serial.println("|--------|--------|--------+--------|");

  // 打印PCA9685 #2
  for(int i=16; i<32; i++) {
    uint16_t pulse = map(channelAngles[i], 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
    Serial.printf("| 0x%02X  | CH%-2d  | %-3d   | %-4d |\n", 
                PCA9685_ADDR2, i-16, channelAngles[i], pulse);
    if((i+1)%4 == 0) Serial.println("+--------+--------+--------+--------+");
  }
}

// 初始化PCA9685
void setupPCA9685(uint8_t addr) {
  for (int i = 0; i < 3; i++) {
    Wire.beginTransmission(addr);
    Wire.write(0x00);
    Wire.write(0x20);
    if (Wire.endTransmission() == 0) break;
    delay(100);
  }
  
  Wire.beginTransmission(addr);
  Wire.write(0x01);
  Wire.write(0x04);
  Wire.endTransmission();
  
  setPWMFreq(addr, 50);
  delay(50);

  Wire.beginTransmission(addr);
  Wire.write(0xFA);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.write(0x00);
  Wire.endTransmission();

  #if DEBUG_MODE
  Serial.printf("PCA9685 @ 0x%02X Initialized\n", addr);
  #endif
}

// 设置PWM频率
void setPWMFreq(uint8_t addr, float freq) {
  freq = constrain(freq, 24, 1526);
  uint8_t prescale = round(25000000.0 / (4096 * freq)) - 1;

  Wire.beginTransmission(addr);
  Wire.write(0x00);
  Wire.write(0x10);
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0xFE);
  Wire.write(prescale);
  Wire.endTransmission();

  Wire.beginTransmission(addr);
  Wire.write(0x00);
  Wire.write(0x20);
  Wire.endTransmission();
}

// 设置舵机角度
void setServoAngle(uint8_t addr, uint8_t channel, uint8_t angle) {
  angle = constrain(angle, 0, 180);
  uint16_t pulse = map(angle, 0, 180, SERVO_MIN_PULSE, SERVO_MAX_PULSE);
  uint16_t ticks = (uint16_t)(pulse * 4096 / 20000.0);

  for (int i = 0; i < 3; i++) {
    Wire.beginTransmission(addr);
    Wire.write(0x06 + 4 * channel);
    Wire.write(0x00);
    Wire.write(0x00);
    Wire.write(ticks & 0xFF);
    Wire.write((ticks >> 8) & 0x0F);
    if (Wire.endTransmission() == 0) break;
    delay(10);
  }
}